package com.neuronrobotics.aim.activecannula;

import java.awt.event.ActionEvent;
import java.awt.event.ActionListener;

import javax.swing.AbstractButton;
import javax.swing.JButton;
import javax.swing.JLabel;
import javax.swing.JPanel;
import javax.swing.JTextField;

import com.neuronrobotics.aim.gui.MainGUI;
import com.neuronrobotics.sdk.genericdevice.GenericPIDDevice;
import com.neuronrobotics.sdk.pid.IPIDEventListener;
import com.neuronrobotics.sdk.pid.PIDConfiguration;
import com.neuronrobotics.sdk.pid.PIDEvent;
import com.neuronrobotics.sdk.pid.PIDLimitEvent;

import net.miginfocom.swing.MigLayout;
import java.awt.Color;
import java.awt.Font;

public class ActiveCannulaControlPanel extends JPanel implements
		IPIDEventListener {
	private static final long serialVersionUID = 1L;
	
	protected static final AbstractButton textFieldResetEncoderInnerTranlsation = null;
	private final JLabel lblOuterTubeTranslation = new JLabel(
			"Outer Tube Translation");
	private final JLabel lblDesiredJointPos = new JLabel("Desired Joint Pos");
	private final JLabel lblActualJointPos = new JLabel("Actual Joint Pos");
	private final JLabel lblTime = new JLabel("Time");
	private final JLabel lblMiddleTubeTranslation = new JLabel(
			"Middle Tube Translation");
	private final JTextField textFieldDesiredOuterTranslation = new JTextField();
	private final JTextField textFieldActualOuterTranslation = new JTextField();
	private final JTextField textFieldTimeOuterTranslation = new JTextField();
	private final JTextField textFieldDesiredMiddleTranslation = new JTextField();
	private final JTextField textFieldActualMiddleTranslation = new JTextField();
	private final JTextField textFieldTimeMiddle = new JTextField();
	private final JLabel lblMiddleTubeRotation = new JLabel(
			"Middle Tube Rotation");
	private final JLabel lblInnerTubeTranslation = new JLabel(
			"Inner Tube Translation");
	private final JTextField textFieldDesiredMiddleRotation = new JTextField();
	private final JTextField textFieldDesiredInnerTranslation = new JTextField();
	private final JTextField textFieldActualMiddleRotation = new JTextField();
	private final JTextField textFieldActualInnerTranslation = new JTextField();
	private final JTextField textFieldTimeInnerTranslation = new JTextField();
	private final JButton btnStop = new JButton("STOP");
	private final JButton btnMoveOuterTranslation = new JButton("Move");
	private final JButton btnMoveMiddle = new JButton("Move");
	private final JButton btnMoveInnerTranslation = new JButton("Move");
	private final JButton btnHome = new JButton("Home");
	private final JButton buttonJogPlusOuterTranslation = new JButton("Jog+");
	private final JButton buttonJogMinusOuterTranslation = new JButton("Jog-");
	private final JButton buttonJogPlusMiddleTranslation = new JButton("Jog+");
	private final JButton buttonJogPlusMiddleRotation = new JButton("Jog+");
	private final JButton buttonJogPlusInnerTranslation = new JButton("Jog+");
	private final JButton buttonJogMinusMiddleTranslation = new JButton("Jog-");
	private final JButton buttonJogMinusMiddleRotation = new JButton("Jog-");
	private final JButton buttonJogMinusInnerTranslation = new JButton("Jog-");
	
	// Linear encoder convention ratio: 1 inch has 4*500 ticks=25.4/(4*500)=0.0127
	private final double encoderTick2mm = 0.0127;
	private final double mm2EncoderTick =78.74;
	
	// Rotary encoder convention ratio: 1 round has 1250 ticks=360/1250/4
	private final double encoderTicks2degree= 0.072;
	private final double degree2EncoderTick= 13.889;
	
	
	private int[] currentJointTicksValues = new int[5];
	private double[] currentJointSIValues = new double[5];
	
	private final JTextField textFieldJogStep = new JTextField();
	private final JLabel labelJogStepUnits = new JLabel("mm/deg");
	
	private int[] desiredJointTicksInteger = new int[5];

	
	public int calibratedOutTranslate=0;
	public int calibratedMiddleTranslate=0;
	public int calibratedMiddleRotate=0;
	public int calibratedInnerTranslate=0;

	private MainGUI kinematicsGUI;
	private final JLabel lblAxis = new JLabel("Axis 1");
	private final JLabel lblAxis_1 = new JLabel("Axis 2");
	private final JLabel lblAxis_2 = new JLabel("Axis 3");
	private final JLabel lblAxis_3 = new JLabel("Axis 4");
	private final JButton btnResetEncoder = new JButton("Reset");
	private final JButton btnPauseOuter = new JButton("Pause");
	private final JButton btnPauseMiddleTranslation = new JButton("Pause");
	private final JButton btnPauseInnerTranslation = new JButton("Pause");
	private final JButton btnStart = new JButton("Start");
	
	public ActiveCannulaControlPanel(MainGUI kinematicsGUI) {
		setKinematicsGUI(kinematicsGUI);
		btnStart.addActionListener(new ActionListener() {
			public void actionPerformed(ActionEvent e) {
				initialize();				
			}
		});
		setLayout(new MigLayout("", "[29px][155px][61px][53px][120px][120px][56px][61px][59px]", "[23px][20px][23px][23px][23px][23px][27px][][]"));
		
		add(btnStart, "cell 1 0,alignx left,aligny top");
		textFieldJogStep.setText("1");
		textFieldJogStep.setColumns(10);
		
		add(textFieldJogStep, "cell 2 1,alignx center,aligny top");		
		add(labelJogStepUnits, "cell 3 1,alignx center,aligny center");
		add(lblDesiredJointPos, "cell 4 1,alignx center,aligny center");
		add(lblActualJointPos, "cell 5 1,alignx center,aligny center");
		add(lblTime, "cell 6 1,alignx center,aligny center");
		add(lblAxis, "cell 0 2,alignx left,aligny center");
		
		lblOuterTubeTranslation.setFont(new Font("Tahoma", Font.PLAIN, 15));
		lblOuterTubeTranslation.setForeground(Color.BLUE);
		add(lblOuterTubeTranslation, "cell 1 2,alignx right,aligny top");
		
		btnMoveOuterTranslation.addActionListener(new ActionListener() {
			public void actionPerformed(ActionEvent e) {	
				
				double seconds = Double.parseDouble(textFieldTimeOuterTranslation.getText());
				double desiredOuterTranslationSI = Double.parseDouble(textFieldDesiredOuterTranslation.getText());				
				int desiredOuterTranslationTicks = (int)(desiredOuterTranslationSI*mm2EncoderTick);	
				
				setDesiredJointTickValue(1,desiredOuterTranslationTicks);		
				runDesiredJointChannelUpdate(1, seconds);	
											
			}
		});
		buttonJogPlusOuterTranslation.addActionListener(new ActionListener() {
			boolean isInitialized= false;
			public void actionPerformed(ActionEvent e) {
				
				double jogStep = Double.parseDouble(textFieldJogStep.getText());				
				double encoderOuterTranslation =getCurrentJointTicksValues()[1]*encoderTick2mm;				
								
				if (isInitialized==true){
					encoderOuterTranslation =Double.parseDouble(textFieldDesiredOuterTranslation.getText()); 
				}				
				isInitialized = true;
									
				double encoderOuterTranslationJog =encoderOuterTranslation+jogStep;	
				textFieldDesiredOuterTranslation.setText(Double.toString(encoderOuterTranslationJog));
				int encoderOuterTranslationJogStepTick =(int) (encoderOuterTranslationJog*mm2EncoderTick);
				
				setDesiredJointTickValue(1,encoderOuterTranslationJogStepTick);						
				runDesiredJointChannelUpdate(1, 0.5);
				
			}
		});
		
				add(buttonJogPlusOuterTranslation, "cell 2 2,alignx left,aligny top");
		buttonJogMinusOuterTranslation.addActionListener(new ActionListener() {
			boolean isInitialized= false;
			public void actionPerformed(ActionEvent e) {
				
				double jogStep = Double.parseDouble(textFieldJogStep.getText());				
				double encoderOuterTranslation =getCurrentJointTicksValues()[1]*encoderTick2mm;				
								
				if (isInitialized==true){
					encoderOuterTranslation =Double.parseDouble(textFieldDesiredOuterTranslation.getText()); 
				}				
				isInitialized = true;
									
				double encoderOuterTranslationJog =encoderOuterTranslation-jogStep;	
				textFieldDesiredOuterTranslation.setText(Double.toString(encoderOuterTranslationJog));
				int encoderOuterTranslationJogStepTick =(int) (encoderOuterTranslationJog*mm2EncoderTick);
				
				setDesiredJointTickValue(1,encoderOuterTranslationJogStepTick);						
				runDesiredJointChannelUpdate(1, 0.5);
				
			}
		});
		
				add(buttonJogMinusOuterTranslation, "cell 3 2,alignx left,aligny top");
		textFieldDesiredOuterTranslation.setText("0");
		textFieldDesiredOuterTranslation.setColumns(10);
		add(textFieldDesiredOuterTranslation, "cell 4 2,growx,aligny center");

		textFieldDesiredOuterTranslation
				.addActionListener(new ActionListener() {
					public void actionPerformed(ActionEvent e) {
					}
				});
		textFieldActualOuterTranslation.setEditable(false);
		textFieldActualOuterTranslation.setText("0");
		textFieldActualOuterTranslation.setColumns(10);

		add(textFieldActualOuterTranslation, "cell 5 2,growx,aligny center");
		textFieldTimeOuterTranslation.setText("3");
		textFieldTimeOuterTranslation.setColumns(10);
		add(textFieldTimeOuterTranslation, "cell 6 2,growx,aligny center");
		btnPauseOuter.addActionListener(new ActionListener() {
			public void actionPerformed(ActionEvent e) {
				double actualOuterTranslation= Double.parseDouble(textFieldActualOuterTranslation.getText());	
			
				textFieldDesiredOuterTranslation.setText(textFieldActualOuterTranslation.getText());	
				setDesiredJointTickValue(1, (int)(actualOuterTranslation*mm2EncoderTick));
				//double seconds = Double.parseDouble(textFieldTimeOuterTranslation.getText()); 
				runDesiredJointChannelUpdate(1, 0.5);				
			}
		});
		
		add(btnPauseOuter, "cell 7 2,alignx left,aligny top");
		add(btnMoveOuterTranslation, "cell 8 2,alignx left,aligny top");
		
		add(lblAxis_1, "cell 0 3,alignx left,aligny center");
				lblInnerTubeTranslation.setForeground(Color.BLUE);
				lblInnerTubeTranslation.setFont(new Font("Tahoma", Font.PLAIN, 15));
				add(lblInnerTubeTranslation, "cell 1 3,alignx center,aligny top");
				buttonJogPlusInnerTranslation.addActionListener(new ActionListener() {
					boolean isInitialized = false;
					public void actionPerformed(ActionEvent e) {
						
						
						double jogStep = Double.parseDouble(textFieldJogStep.getText());
						double encoderInnerTranslation =getCurrentJointTicksValues()[2]*encoderTick2mm;	
						
						if (isInitialized==true){
							encoderInnerTranslation =Double.parseDouble(textFieldDesiredInnerTranslation.getText()); 
						}
						
						isInitialized = true;
						
						double encoderInnerTranslationJog =encoderInnerTranslation+jogStep;		
						textFieldDesiredInnerTranslation.setText(Double.toString(encoderInnerTranslationJog));
						int encoderInnerTranslationJogTick =(int) (encoderInnerTranslationJog*mm2EncoderTick);
						
						setDesiredJointTickValue(2,encoderInnerTranslationJogTick);						
						runDesiredJointChannelUpdate(2, 0.5);
					}
				});
				
						add(buttonJogPlusInnerTranslation, "cell 2 3,alignx left,aligny top");
		buttonJogMinusInnerTranslation.addActionListener(new ActionListener() {
			boolean isInitialized = false;
			public void actionPerformed(ActionEvent e) {
				
				
				double jogStep = Double.parseDouble(textFieldJogStep.getText());
				double encoderInnerTranslation =getCurrentJointTicksValues()[2]*encoderTick2mm;	
				
				if (isInitialized==true){
					encoderInnerTranslation =Double.parseDouble(textFieldDesiredInnerTranslation.getText()); 
				}
				
				isInitialized = true;
				
				double encoderInnerTranslationJog =encoderInnerTranslation-jogStep;		
				textFieldDesiredInnerTranslation.setText(Double.toString(encoderInnerTranslationJog));
				int encoderInnerTranslationJogTick =(int) (encoderInnerTranslationJog*mm2EncoderTick);
				
				setDesiredJointTickValue(2,encoderInnerTranslationJogTick);						
				runDesiredJointChannelUpdate(2, 0.5);
				
				
			}
		});
		
				add(buttonJogMinusInnerTranslation, "cell 3 3,alignx left,aligny top");
		
				textFieldDesiredInnerTranslation.addActionListener(new ActionListener() {
					public void actionPerformed(ActionEvent e) {
					}
				});
				textFieldDesiredInnerTranslation.setText("0");
				textFieldDesiredInnerTranslation.setColumns(10);
				add(textFieldDesiredInnerTranslation, "cell 4 3,growx,aligny center");
		textFieldActualInnerTranslation.setEditable(false);
		textFieldActualInnerTranslation.setText("0");
		textFieldActualInnerTranslation.setColumns(10);
		add(textFieldActualInnerTranslation, "cell 5 3,growx,aligny center");
		textFieldTimeInnerTranslation.setText("3");
		textFieldTimeInnerTranslation.setColumns(10);
		add(textFieldTimeInnerTranslation, "cell 6 3,growx,aligny center");
		btnPauseInnerTranslation.addActionListener(new ActionListener() {
			public void actionPerformed(ActionEvent e) {
				
				double actualInnerTranslation= Double.parseDouble(textFieldActualInnerTranslation.getText());				
				textFieldDesiredInnerTranslation.setText(textFieldActualInnerTranslation.getText());	
				setDesiredJointTickValue(2, (int)(actualInnerTranslation*mm2EncoderTick));
				//double seconds = Double.parseDouble(textFieldTimeInnerTranslation.getText()); 
				runDesiredJointChannelUpdate(2, 0.5);
							
			}
		});
		
		add(btnPauseInnerTranslation, "cell 7 3,alignx left,aligny top");
		
				btnMoveInnerTranslation.addActionListener(new ActionListener() {
					public void actionPerformed(ActionEvent e) {
						
						double seconds = Double.parseDouble(textFieldTimeInnerTranslation.getText());
						double desiredInnerTranslationSI = Double.parseDouble(textFieldDesiredOuterTranslation.getText());				
						int desiredInnerTranslationTicks = (int)(desiredInnerTranslationSI*mm2EncoderTick);	
						
						setDesiredJointTickValue(2,desiredInnerTranslationTicks);		
						runDesiredJointChannelUpdate(2, seconds);
						
					}
				});
				
						add(btnMoveInnerTranslation, "cell 8 3,alignx left,aligny top");
		
		add(lblAxis_2, "cell 0 4,alignx left,aligny center");
						lblMiddleTubeTranslation.setFont(new Font("Tahoma", Font.PLAIN, 15));
						lblMiddleTubeTranslation.setForeground(Color.BLUE);
						add(lblMiddleTubeTranslation, "cell 1 4,alignx left,aligny top");
						buttonJogPlusMiddleTranslation.addActionListener(new ActionListener() {
							boolean isInitialized = false;
							public void actionPerformed(ActionEvent e) {
								
								double jogStep = Double.parseDouble(textFieldJogStep.getText());
								double encoderMiddleTranslation =getCurrentJointTicksValues()[3]*encoderTick2mm;	
								
								if (isInitialized==true){
									encoderMiddleTranslation =Double.parseDouble(textFieldDesiredMiddleTranslation.getText()); 
								}
								
								isInitialized = true;
								
								double encoderMiddleTranslationJog =encoderMiddleTranslation+jogStep;		
								textFieldDesiredMiddleTranslation.setText(Double.toString(encoderMiddleTranslationJog));
								int encoderMiddleTranslationJogStepTick =(int) (encoderMiddleTranslationJog*mm2EncoderTick);
								
								setDesiredJointTickValue(3,encoderMiddleTranslationJogStepTick);						
								runDesiredJointChannelUpdate(3, 0.5);
								
							}
						});
						
								add(buttonJogPlusMiddleTranslation, "cell 2 4,alignx left,aligny top");
				buttonJogMinusMiddleTranslation.addActionListener(new ActionListener() {
					boolean isInitialized = false;
					public void actionPerformed(ActionEvent e) {
						
						double jogStep = Double.parseDouble(textFieldJogStep.getText());
						double encoderMiddleTranslation =getCurrentJointTicksValues()[3]*encoderTick2mm;	
						
						if (isInitialized==true){
							encoderMiddleTranslation =Double.parseDouble(textFieldDesiredMiddleTranslation.getText()); 
						}
						
						isInitialized = true;
						
						double encoderMiddleTranslationJog =encoderMiddleTranslation-jogStep;		
						textFieldDesiredMiddleTranslation.setText(Double.toString(encoderMiddleTranslationJog));
						int encoderMiddleTranslationJogStepTick =(int) (encoderMiddleTranslationJog*mm2EncoderTick);
						
						setDesiredJointTickValue(3,encoderMiddleTranslationJogStepTick);						
						runDesiredJointChannelUpdate(3, 0.5);
										
					}
				});
				
						add(buttonJogMinusMiddleTranslation, "cell 3 4,alignx left,aligny top");
		
				textFieldDesiredMiddleTranslation
						.addActionListener(new ActionListener() {
							public void actionPerformed(ActionEvent e) {
							}
						});
				textFieldDesiredMiddleTranslation.setText("0");
				textFieldDesiredMiddleTranslation.setColumns(10);
				add(textFieldDesiredMiddleTranslation, "cell 4 4,growx,aligny center");
		textFieldActualMiddleTranslation.setEditable(false);
		textFieldActualMiddleTranslation.setText("0");
		textFieldActualMiddleTranslation.setColumns(10);
		add(textFieldActualMiddleTranslation, "cell 5 4,growx,aligny center");
		textFieldTimeMiddle.setText("3");
		textFieldTimeMiddle.setColumns(10);
		add(textFieldTimeMiddle, "cell 6 4,growx,aligny center");
		btnPauseMiddleTranslation.addActionListener(new ActionListener() {
			public void actionPerformed(ActionEvent e) {
				double actualMiddleTranslation= Double.parseDouble(textFieldActualMiddleTranslation.getText());				
				textFieldDesiredMiddleTranslation.setText(textFieldActualMiddleTranslation.getText());	
				setDesiredJointTickValue(3, (int)(actualMiddleTranslation*mm2EncoderTick));
				
				double actualMiddleRotation= Double.parseDouble(textFieldActualMiddleTranslation.getText());				
				textFieldDesiredMiddleRotation.setText(textFieldActualMiddleRotation.getText());	
				setDesiredJointTickValue(4, (int)(actualMiddleRotation*degree2EncoderTick));
				
				//double seconds = Double.parseDouble(textFieldTimeMiddle.getText()); 
				runDesiredJointChannelUpdate(3, 0.5);
				runDesiredJointChannelUpdate(4, 0.5);
				
				
			}
		});
		
		add(btnPauseMiddleTranslation, "cell 7 4,alignx left,aligny top");
		btnMoveMiddle.addActionListener(new ActionListener() {
			public void actionPerformed(ActionEvent e) {
				
				double seconds = Double.parseDouble(textFieldTimeMiddle.getText());
				double desiredMiddleTranslationSI = Double.parseDouble(textFieldDesiredMiddleTranslation.getText());
				int desiredMiddleTranslationTicks = (int)(desiredMiddleTranslationSI*mm2EncoderTick);	
				setDesiredJointTickValue(3,desiredMiddleTranslationTicks);	
				
				double desiredMiddleRotationSI = Double.parseDouble(textFieldDesiredMiddleRotation.getText());
				int desiredMiddleRotationTicks = (int)(desiredMiddleRotationSI*degree2EncoderTick);	
				setDesiredJointTickValue(4,desiredMiddleRotationTicks);	
								
				runDesiredJointChannelUpdate(3, seconds);
				runDesiredJointChannelUpdate(4, seconds);
				
			}
		});
		add(btnMoveMiddle, "cell 8 4,alignx left,aligny top");
		
		add(lblAxis_3, "cell 0 5,alignx left,aligny center");
		lblMiddleTubeRotation.setForeground(Color.BLUE);
		lblMiddleTubeRotation.setFont(new Font("Tahoma", Font.PLAIN, 15));
		add(lblMiddleTubeRotation, "cell 1 5,alignx center,aligny top");
				buttonJogPlusMiddleRotation.addActionListener(new ActionListener() {
					boolean isInitialized = false;
					public void actionPerformed(ActionEvent e) {
										
						double jogStep = Double.parseDouble(textFieldJogStep.getText());
						double encoderMiddleRotation =getCurrentJointTicksValues()[4]*encoderTicks2degree;	
						
						if (isInitialized==true){
							encoderMiddleRotation =Double.parseDouble(textFieldDesiredMiddleRotation.getText()); 
						}
						
						isInitialized = true;
						
						double encoderMiddleRotationJog =encoderMiddleRotation+jogStep;		
						textFieldDesiredMiddleRotation.setText(Double.toString(encoderMiddleRotationJog));
						int encoderMiddleRotationJogStepTick =(int) (encoderMiddleRotationJog*degree2EncoderTick);
						
						setDesiredJointTickValue(4,encoderMiddleRotationJogStepTick);						
						runDesiredJointChannelUpdate(4, 0.5);
						
					}
				});
				
						add(buttonJogPlusMiddleRotation, "cell 2 5,alignx left,aligny top");
				buttonJogMinusMiddleRotation.addActionListener(new ActionListener() {
					boolean isInitialized = false;
					public void actionPerformed(ActionEvent e) {
						
						
						double jogStep = Double.parseDouble(textFieldJogStep.getText());
						double encoderMiddleRotation =getCurrentJointTicksValues()[4]*encoderTicks2degree;	
						
						if (isInitialized==true){
							encoderMiddleRotation =Double.parseDouble(textFieldDesiredMiddleRotation.getText()); 
						}
						
						isInitialized = true;
						
						double encoderMiddleRotationJog =encoderMiddleRotation-jogStep;		
						textFieldDesiredMiddleRotation.setText(Double.toString(encoderMiddleRotationJog));
						int encoderMiddleRotationJogStepTick =(int) (encoderMiddleRotationJog*degree2EncoderTick);
						
						setDesiredJointTickValue(4,encoderMiddleRotationJogStepTick);						
						runDesiredJointChannelUpdate(4, 0.5);
						
						
					}
				});
				
						add(buttonJogMinusMiddleRotation, "cell 3 5,alignx left,aligny top");
		
				textFieldDesiredMiddleRotation.addActionListener(new ActionListener() {
					public void actionPerformed(ActionEvent e) {
					}
				});
				textFieldDesiredMiddleRotation.setText("0");
				textFieldDesiredMiddleRotation.setColumns(10);
				add(textFieldDesiredMiddleRotation, "cell 4 5,growx,aligny center");
						textFieldActualMiddleRotation.setEditable(false);
						textFieldActualMiddleRotation.setText("0");
						textFieldActualMiddleRotation.setColumns(10);
						add(textFieldActualMiddleRotation, "cell 5 5,growx,aligny center");
								btnResetEncoder.addActionListener(new ActionListener() {
									public void actionPerformed(ActionEvent e) {

										// 1 step, resent encoder, all encoders are cleared to zero
										// this assume that encoders give relative position		
										
										getDevice().ResetPIDChannel(1, 0);
										getDevice().ResetPIDChannel(2, 0);
										getDevice().ResetPIDChannel(3, 0);
										getDevice().ResetPIDChannel(4, 0);
										
//										// 2 step, set PID set point 
//										setDesiredJointTickValue(1, 0);
//										setDesiredJointTickValue(2, 0);
//										setDesiredJointTickValue(3, 0);
//										setDesiredJointTickValue(4, 0);
										
//										//3 step, move to zero
//										runDesiredJointUpdate(1.5);
											
										
														
										//resumePID();															
										/*		setDesiredJointTickValue(1, calibratedOutTranslate);
												setDesiredJointTickValue(2, calibratedMiddleTranslate);
												setDesiredJointTickValue(3, calibratedMiddleRotate);
												setDesiredJointTickValue(4, calibratedInnerTranslate);
												calibratedOutTranslate= (int)(Double.parseDouble(textFieldActualOuterTranslation.getText())*mm2EncoderTick);
												calibratedMiddleTranslate= (int)(Double.parseDouble(textFieldActualMiddleTranslation.getText())*mm2EncoderTick);
												calibratedMiddleRotate= (int)(Double.parseDouble(textFieldActualMiddleRotation.getText())*degree2EncoderTick);
												calibratedInnerTranslate= (int)(Double.parseDouble(textFieldActualInnerTranslation.getText())*mm2EncoderTick);								
												*/								
									}
								});
								
								add(btnResetEncoder, "cell 2 8,alignx left,aligny bottom");
								btnHome.addActionListener(new ActionListener() {
									public void actionPerformed(ActionEvent e) {

										setDesiredJointTickValue(1, 0);
										setDesiredJointTickValue(2, 0);
										setDesiredJointTickValue(3, 0);
										setDesiredJointTickValue(4, 0);
										
										runDesiredJointUpdate(3);
									}
								});
								btnHome.setFont(new Font("Tahoma", Font.PLAIN, 15));
								add(btnHome, "cell 4 8,alignx center,aligny top");
								btnStop.setForeground(Color.RED);
								btnStop.setFont(new Font("Tahoma", Font.BOLD, 15));
								btnStop.setBounds(91, 332, 80, 55);
								
										btnStop.addActionListener(new ActionListener() {
											public void actionPerformed(ActionEvent e) {								
												stopPID();
												
											}
										});
										add(btnStop, "cell 5 8,alignx center,aligny top");

	}

	
	private void initialize() {
		kinematicsGUI.getDevice().addPIDEventListener(this);
		
		double temp= kinematicsGUI.getDevice().GetPIDPosition(0)*encoderTick2mm;
		textFieldActualOuterTranslation.setText(Double.toString(temp));
		
		temp= kinematicsGUI.getDevice().GetPIDPosition(1)*encoderTick2mm;
		textFieldActualMiddleTranslation.setText(Double.toString(temp));
		
		temp= kinematicsGUI.getDevice().GetPIDPosition(3)*encoderTick2mm;
		textFieldActualMiddleRotation.setText(Double.toString(temp));
		
		temp= kinematicsGUI.getDevice().GetPIDPosition(4)*encoderTick2mm;
		textFieldActualInnerTranslation.setText(Double.toString(temp));
	}


	@Override
	public void onPIDEvent(PIDEvent e) {
		System.err
		.println("Got a PID event in Kinematics GUI #" + e.getGroup()
				+ " value: " + e.getValue() + " timestamp: "
				+ e.getTimeStamp());
		// getCurrentJointTicksValues() is 5x1 vector
		getCurrentJointTicksValues()[e.getGroup()] = e.getValue();					
		updateRobotJointDisplay(getCurrentJointTicksValues());		
	}

	@Override
	public void onPIDLimitEvent(PIDLimitEvent e) {
		
	}

	@Override
	public void onPIDReset(int group, int currentValue) {
		setDesiredJointTickValue(group, currentValue);
		switch (group){
		case 0: 
			break;
		
		case 1: 
			textFieldDesiredOuterTranslation.setText(new Double(currentValue).toString());
			textFieldActualOuterTranslation.setText(new Double(currentValue).toString()); 
			break;
			
		case 2: 
			textFieldDesiredInnerTranslation.setText(new Double(currentValue).toString());
			textFieldActualInnerTranslation.setText(new Double(currentValue).toString()); 
			break;
			
		case 3: 
			textFieldDesiredMiddleTranslation.setText(new Double(currentValue).toString());
			textFieldActualMiddleTranslation.setText(new Double(currentValue).toString()); 
			break;
			
		case 4: 
			textFieldDesiredMiddleRotation.setText(new Double(currentValue).toString());
			textFieldActualMiddleRotation.setText(new Double(currentValue).toString()); 
			break;
			
		}
		
	}
	
	public void updateRobotJointDisplay (int[] currentJointTicks){
		
		double outerTranslation = (getCurrentJointTicksValues()[1]-calibratedOutTranslate)*encoderTick2mm;
		textFieldActualOuterTranslation.setText(Double.toString(outerTranslation));
		
		double middleTranslation =(getCurrentJointTicksValues()[3]-calibratedMiddleTranslate)*encoderTick2mm;
		textFieldActualMiddleTranslation.setText(Double.toString(middleTranslation));
		
		double middleRotation = (getCurrentJointTicksValues()[4]-calibratedMiddleRotate)*encoderTicks2degree;
		textFieldActualMiddleRotation.setText(Double.toString(middleRotation));
		
		double innerTranslation = (getCurrentJointTicksValues()[2]-calibratedInnerTranslate)*encoderTick2mm;
		textFieldActualInnerTranslation.setText(Double.toString(innerTranslation));
		
//		// this would set up the errors
//		double outerTranslationError= Double.parseDouble(textFieldDesiredOuterTranslation.getText())-outerTranslation;		
	}

	public void setCurrentJointTicksValues(int[] currentJointTicksValues) {
		this.currentJointTicksValues = currentJointTicksValues;
	}

	public int[] getCurrentJointTicksValues() {
		return currentJointTicksValues;
	}
	
	
	public double[] getCurrentJointSIValues() {
		return currentJointSIValues;
	}
	
	public void setCurrentJointChannelSIValues(int channel , double currentJointSIValues) {
		this.currentJointSIValues[channel] = currentJointSIValues;
	}
	
	
	public GenericPIDDevice getDevice() {
		return kinematicsGUI.getDevice();
	}

	public synchronized void runDesiredJointUpdate(double seconds) {
		// 1 step: update desired joint
		// reference the jointSpaceVector (integer: encoder ticks)
//		setDesiredJointTickValue(0,desiredJointTicksInteger[0]);
//		setDesiredJointTickValue(1,desiredJointTicksInteger[1]);//-calibratedOutTranslate);
//		setDesiredJointTickValue(2,desiredJointTicksInteger[2]);//-calibratedMiddleTranslate);
//		setDesiredJointTickValue(3,desiredJointTicksInteger[3]);//-calibratedMiddleRotate);	
//		setDesiredJointTickValue(4,desiredJointTicksInteger[4]);//-calibratedInnerTranslate);	
		
		System.err.println("Updating motors");
		getDevice().SetPIDSetPoint(0, getDesiredJointTicksValues()[0],seconds);
		getDevice().SetPIDSetPoint(1, getDesiredJointTicksValues()[1],seconds);
		getDevice().SetPIDSetPoint(2, getDesiredJointTicksValues()[2],seconds);
		getDevice().SetPIDSetPoint(3, getDesiredJointTicksValues()[3],seconds);
		getDevice().SetPIDSetPoint(4, getDesiredJointTicksValues()[4],seconds);	
	}
	
	
	public synchronized void runDesiredJointChannelUpdate(int channel, double seconds) {
		int calibrated=0;
	
		/* switch (channel) {
		 case 0:  calibrated = 0;     //  break;
         case 1:  calibrated = calibratedOutTranslate;    //   break;
         case 2:  calibrated = calibratedMiddleTranslate;   //   break;
         case 3:  calibrated = calibratedMiddleRotate;       //  break;
         case 4:  calibrated = calibratedInnerTranslate;      //   break;         
         default: calibrated = 0; //break;
     }*/
		setDesiredJointTickValue(channel,desiredJointTicksInteger[channel]+calibrated);		
		System.err.println("Updatging motors");
		getDevice().SetPIDSetPoint(channel, getDesiredJointTicksValues()[channel],seconds);
		
	}
	
	
	public void setDesiredJointTickValue(int group, int target) {
		getDesiredJointTicksValues()[group] = target;
	}
	
	public int[] getDesiredJointTicksValues() {
		return desiredJointTicksInteger;
	}
	
	
	public void stopPID(){
		System.out.println("Calling PID stop");
		for (int i=0;i<5;i++){
			try{
				//PIDConfiguration conf = new PIDConfiguration();
				PIDConfiguration conf = getDevice().getPIDConfiguration(i);
				conf.setEnabled(false);
				getDevice().ConfigurePIDController(conf);
				System.out.println("Stopping group "+i);
			}catch(Exception ex){
				ex.printStackTrace();
			}
		}	
	}
	
	public void stopPIDChannel(int channel){
				try{
					PIDConfiguration conf = new PIDConfiguration();
					conf.setEnabled(false);
					conf.setGroup(channel);
					getDevice().ConfigurePIDController(conf);
					
				}catch(Exception ex){
					ex.printStackTrace();
				}
	}
		
		
		
	public void resumePID(){
			for (int i=0;i<5;i++){
				PIDConfiguration conf = new PIDConfiguration();
				conf.setEnabled(true);
				conf.setGroup(i);
				getDevice().ConfigurePIDController(conf);
			}
	}
	
	public void setKinematicsGUI(MainGUI kinematicsGUI) {
		this.kinematicsGUI = kinematicsGUI;
	}
	
	public double[] roundDouble2TwoDigits(double[] longDigitsDouble) {
		for (int i = 0; i < longDigitsDouble.length; i++) {
			longDigitsDouble[i] = Math.round(longDigitsDouble[i] * 100) / 100.00;
		}
		return longDigitsDouble;
	}
}
